import cv2
from picamera2 import Picamera2
import time
import numpy as np
import math
import serial

startChar = b'-'
stopChar = b'='
data = b''
ser = serial.Serial(
    port='/dev/ttyS0',
    baudrate=57600  # ,
    #     parity=serial.PARITY_NONE,
    #     stopbits=serial.STOPBITS_ONE,
    #     bytesize=serial.EIGHTBITS
)
ser.close()
ser.open()
response = 0
st = 0
message = "@ello="

# picam2 = cv2.VideoCapture(0)
picam2 = Picamera2()
dispW = 640
dispH = 480
picam2.exposure_mode = 'off'
picam2.exposure_compensation = -2
picam2.awb_mode = 'off'
picam2.awb_gains = (1.0, 1.0)
picam2.iso = 100
picam2.shutter_speed = 1000
picam2.brightness = 10
picam2.framerate = 20
picam2.preview_configuration.main.size = (dispW, dispH)
picam2.preview_configuration.main.format = "RGB888"
# picam2.preview_configuration.controls.FrameRate = 30
picam2.preview_configuration.align()
picam2.configure("preview")
picam2.start()
fps = 0
pos = (40, 60)
font = cv2.FONT_HERSHEY_SIMPLEX
height = 1.5
weight = 2
myColor = (0, 0, 255)


def onTrack1(val):
    global hueLow
    hueLow = val
    print('Hue Low', hueLow)


def onTrack2(val):
    global hueHigh
    hueHigh = val
    print('Hue High', hueHigh)


def onTrack3(val):
    global satLow
    satLow = val
    print('Sat Low', satLow)


def onTrack4(val):
    global satHigh
    satHigh = val
    print('Sat High', satHigh)


def onTrack5(val):
    global valLow
    valLow = val
    print('Val Low', valLow)


def onTrack6(val):
    global valHigh
    valHigh = val
    print('Val High', valHigh)


cv2.namedWindow('myTracker')

cv2.createTrackbar('Hue Low', 'myTracker', 10, 179, onTrack1)
cv2.createTrackbar('Hue High', 'myTracker', 20, 179, onTrack2)
cv2.createTrackbar('Sat Low', 'myTracker', 100, 255, onTrack3)
cv2.createTrackbar('Sat High', 'myTracker', 2555, 255, onTrack4)
cv2.createTrackbar('Val Low', 'myTracker', 100, 255, onTrack5)
cv2.createTrackbar('Val High', 'myTracker', 255, 255, onTrack6)

while True:
    byte = ser.read()
    if st == 0:
        if byte == startChar:
            st = 1
    if st == 1:
        if byte == stopChar:
            st = 2
        data += byte
    if st == 2:
        response = data
        #         ser.write(response)
        st = 0
    if response:
        print("Received:", response)

        tStart = time.time()
        frame = picam2.capture_array()
        #     ret, frame = picam2.read()
        blur = cv2.GaussianBlur(frame, (5, 5), 0)
        # frame = cv2.flip(frame, -1)
        frameHSV = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV)
        cv2.putText(frame, str(int(fps)) + ' FPS', pos, font, 1, myColor, weight)
        #     lowerBound = np.array([hueLow, satLow, valLow])
        #     upperBound = np.array([hueHigh, satHigh, valHigh])
        lowerBound = np.array([0, 0, 255])
        upperBound = np.array([30, 30, 255])
        myMask = cv2.inRange(frameHSV, lowerBound, upperBound)
        myMaskSmall = cv2.resize(myMask, (int(dispW / 2), int(dispH / 2)))
        myObject = cv2.bitwise_and(frame, frame, mask=myMask)
        myObjectSmall = cv2.resize(myObject, (int(dispW / 2), int(dispH / 2)))
        xw = 0
        contours, junk = cv2.findContours(myMask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
        if len(contours) > 0:
            contours = sorted(contours, key=lambda x: cv2.contourArea(x), reverse=True)
            # cv2.drawContours(frame,contours,-1,(255,0,0),3)
            contour = contours[0]
            x, y, w, h = cv2.boundingRect(contour)
            cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 3)
            xw = x + w // 2
        ##/////////////////////////////
        #     retval1,dst1=cv2.threshold(myObject, 0, 255, cv2.THRESH_OTSU)
        #     mask=dst1[x:x+w,y:y+h]
        #     for i in range(2*x,2*(x+w)):
        #         for j in range(2*y,2*(y+h)):
        #             blur[i,j]=blur[i,j]//4
        lowerBound = np.array([0, 0, 60])
        upperBound = np.array([110, 255, 255])
        H, S, dst = cv2.split(frameHSV)
        _, dst = cv2.threshold(dst, 100, 255, cv2.THRESH_BINARY)
        dst = cv2.merge([H, S, dst])
        dst = cv2.cvtColor(dst, cv2.COLOR_HSV2BGR)
        gray = cv2.cvtColor(dst, cv2.COLOR_BGR2GRAY)
        #     gray = cv2.cvtColor(blur, cv2.COLOR_BGR2GRAY)
        #     gray = cv2.bitwise_and(gray, gray, mask=myObject)
        retval, dst = cv2.threshold(gray, 0, 255, cv2.THRESH_OTSU)
        dst = cv2.dilate(dst, None, iterations=2)
        height, width = dst.shape[:2]  # 480 640
        li = 200
        ri = 390
        color1 = dst[:, li]
        color2 = dst[:, ri]

        black_white1_judge = np.sum(color1 == 255)
        black_white2_judge = np.sum(color2 == 255)
        black_index1 = np.where(color1 == 0)
        black_index2 = np.where(color2 == 0)
        black_count1 = np.sum(color1 == 0)
        black_count2 = np.sum(color2 == 0)
        #     if black_count1>30:black_count1=30
        #     if black_count2>30:black_count2=30
        center1 = 0
        center2 = 0
        if black_white1_judge < 450 and black_white2_judge < 450:
            center1 = (black_index1[0][black_count1 - 1])  # + black_index1[0][black_count1-40]) / 2
            #         direction1 = center1 - 302
            center2 = (black_index2[0][black_count2 - 1])  # + black_index2[0][black_count2]) / 2
            #         direction2 = center2 - 302
            # print("center1:", center1, "center2:", center2)
            #         angle = '%.2f' % (math.degrees(np.arctan(100 / (direction2 - direction1))))
            cv2.line(frame, (li, int(center1) - 30), (ri, int(center2) - 30), color=(255, 0, 0), thickness=3)
        #         cv2.imshow("frame", frame)
        yh = (center2 + center1) // 2 - 20
        #     cv2.imshow("dst", dst)
        cv2.namedWindow("dst", cv2.WINDOW_NORMAL)
        cv2.imshow("dst", dst)
        cv2.resizeWindow("dst", 400, 300)
        message = '@' + 'x' + str(xw) + '='
        ser.write(message.encode())
        time.sleep(0.1)
        message = '@' + 'y' + str(yh) + '='
        ser.write(message.encode())

        cv2.putText(frame, str(int(yh)) + ' y', (20, 400), font, 0.6, myColor, weight)
        cv2.putText(frame, str(int(xw)) + ' x', (90, 400), font, 0.6, myColor, weight)
        cv2.namedWindow("Camera", cv2.WINDOW_NORMAL)
        cv2.imshow("Camera", frame)
        cv2.resizeWindow("Camera", 400, 300)
        cv2.moveWindow("Camera", 200, 100)
        # cv2.imshow('Mask', myMaskSmall)
        cv2.namedWindow("Object", cv2.WINDOW_NORMAL)
        cv2.imshow('Object', myObjectSmall)
        cv2.resizeWindow("Object", 600, 400)
        cv2.moveWindow("Object", 700, 100)

        if cv2.waitKey(1) == ord('q'):
            break
        tEnd = time.time()
        loopTime = tEnd - tStart
        fps = .9 * fps + .1 * (1 / loopTime)
cv2.destroyAllWindows()
ser.close()
